Прямая задача кинематики PUMA

In [1]:
from matplotlib import pyplot as plt
from matplotlib import animation
import numpy as np
from IPython.display import HTML
%matplotlib notebook
In [2]:
from kinematics import Vector, Quaternion, Transform
import graphics

Решение прямой задачи кинематики для манипулятора PUMA

Манипулятор кинематической схемы PUMA обладает шестью степенями подвижности.

внешний виж PUMA

Его можно условно разделить на сегменты, соединияюще между собой:

  • основание
  • плечо
  • локоть
  • кисть
  • фланец

кинематическая схема PUMA

Длина этих сегментов определена в таблице:

Пара длина
основание - плечо $l_0$
плечо - локоть $l_1$
локоть - кисть $l_2$
кисть - фланец $l_3$
In [3]:
def puma_chain(q, l, math_source=np):
    base = Transform.identity()
    shoulder = base + Transform(
        Vector(0, 0, l[0]),
        Quaternion.from_angle_axis(q[0], Vector(0, 0, 1), math_source) * 
        Quaternion.from_angle_axis(q[1], Vector(0, 1, 0), math_source)
    )
    elbow = shoulder + Transform(
        Vector(0, 0, l[1]),
        Quaternion.from_angle_axis(q[2], Vector(0, 1, 0), math_source)
    )
    wrist = elbow + Transform(
        Vector(0, 0, l[2]),
        Quaternion.from_angle_axis(q[3], Vector(0, 0, 1), math_source) *
        Quaternion.from_angle_axis(q[4], Vector(0, 1, 0), math_source)
    )
    flange = wrist + Transform(
        Vector(0, 0, l[3]),
        Quaternion.from_angle_axis(q[5], Vector(0, 0, 1), math_source)
    )
    return [base, shoulder, elbow, wrist, flange]

Зададим изменение обобщенных координат:

In [52]:
def puma_q(t, total):
    #omega = t / total * np.pi * 2
    a = (t - total) / 2
    omega = (-np.cos(3.14 / total * t) + 1.0)*5

    return [
        np.pi / 4 * np.cos(omega),
        np.pi / 6* np.sin(omega),
        np.pi / 2,
        np.pi*3/4,
        np.pi *np.sin(omega)/ 6,
        0
    ]

Зададим длины звеньев:

In [53]:
puma_l = [1, 2, 1, 0.5]
In [54]:
puma_fig = plt.figure()
ax = puma_fig.add_subplot(projection="3d")
ax.set_xlim([-2, 2]); ax.set_ylim([-2, 2]); ax.set_zlim([0, 4])
lines, = ax.plot([], [], [], color="#000000")
graphics.axis(ax, Transform.identity(), 2)
r, g, b = graphics.axis(ax, Transform.identity(), 1)

total = 100

def animate(frame):
    chain = puma_chain(puma_q(frame, total), puma_l)
    (x, y, z) = graphics.chain_to_points(chain)
    lines.set_data_3d(x, y, z)
    global r, g, b
    r.remove(); g.remove(); b.remove()
    r, g, b = graphics.axis(ax, chain[-1], 0.5)

    
animate(0)
fps = 25
puma_ani = animation.FuncAnimation(
    puma_fig,
    animate,
    frames=total,
    interval=1000.0/fps
)
In [55]:
HTML(puma_ani.to_jshtml())
Out[55]:

Отображение траектории

In [56]:
t_end = 10
step = 0.1
t = np.arange(0, t_end, step)
chain = puma_chain(puma_q(t, t_end), puma_l)
x, y, z = graphics.chain_to_points(chain)

fig = plt.figure()
ax = fig.add_subplot(projection="3d")
ax.set_xlim([-2, 2]); ax.set_ylim([-2, 2]); ax.set_zlim([0, 4])
ax.plot(x[2], y[2], z[2], color="#c0c0c0")
ax.plot(x[3], y[3], z[3], color="#c0c0c0")
ax.plot(x[4], y[4], z[4], color="#000000")
graphics.axis(ax, Transform.identity(), 2)
fig.show()
print(
    "X координата менялась в диапазоне от",
    np.min(x[-1]),
    "до",
    np.max(x[-1]),
        "\nY координата менялась в диапазоне от",
    np.min(y[-1]),
    "до",
    np.max(y[-1]),
        "\nZ координата менялась в диапазоне от",
    np.min(z[-1]),
    "до",
    np.max(z[-1])
)
X координата менялась в диапазоне от 0.32187772587569363 до 2.334022433636939 
Y координата менялась в диапазоне от -1.1898168635068487 до 1.3119564153188643 
Z координата менялась в диапазоне от 2.168650051278921 до 3.2961913036419443

Измерение скорости движения

Численно продиффиринцируем положение всех точек траектории

In [58]:
t_end = 10
step = 0.01
t = np.arange(0, t_end, step)
chain = puma_chain(puma_q(t, t_end), puma_l)
end = chain[-1]
velocity_x = np.diff(end.translation.x) / step
velocity_y = np.diff(end.translation.y) / step
velocity_z = np.diff(end.translation.z) / step
velocity_total = (velocity_x ** 2 + velocity_y ** 2 + velocity_z ** 2) ** 0.5

fig = plt.figure()
ax = fig.add_subplot()

ax.plot(t[:-1], velocity_x, color="#ff0000", label="$V_x$")
ax.plot(t[:-1], velocity_y, color="#00ff00", label="$V_y$")
ax.plot(t[:-1], velocity_z, color="#0000ff", label="$V_z$")
ax.plot(t[:-1], velocity_total, color="#000000", label="$V$")
fig.legend()
fig.show()

index_max = np.argmax(velocity_total)
vx_max = np.argmax(velocity_x)
vy_max = np.argmax(velocity_y)
vz_max = np.argmax(velocity_z)
print(
    "Максимальная скорость была",    velocity_total[index_max],    "в момент",    index_max * step,    "сек\n",    
    "Максимальная скорость по оси X была",    velocity_x[vx_max],    "в момент",    vx_max * step,    "сек\n",
    "Максимальная скорость по оси Y была",    velocity_y[vy_max],    "в момент",    vy_max * step,    "сек\n",
    "Максимальная скорость по оси Y была",    velocity_z[vz_max],    "в момент",    vz_max * step,    "сек\n"
)
Максимальная скорость была 2.379054911697325 в момент 6.8500000000000005 сек
 Максимальная скорость по оси X была 1.5236329096548484 в момент 6.33 сек
 Максимальная скорость по оси Y была 1.689859544573824 в момент 5.55 сек
 Максимальная скорость по оси Y была 1.0259567021000837 в момент 3.5100000000000002 сек

Самостоятельные задания

Для своего закона изменения обобщенных координат

  • Оцените диапазон $X$, $Y$ и $Z$ координат для данной траектории;
  • Оцените максимальную скорость по осям;
  • Оцените абсолютную максимальную скорость.
In [ ]: